#include "platform.h"
#include "riscv_asm.h"
#include "common.S"

# size of each hart's stack is 1024 bytes
.equ STACK_SIZE, 1024
.globl _start

.section .text.boot

_start:
	csrr	t0, mhartid
	mv	tp, t0
	bnez	t0, wait

	# clear bss section to zero
	la	a0, _bss_start
	la	a1, _bss_end
	bgeu	a0, a1, 2f
1:
	sw	zero, (a0)
	addi	a0, a0, 4
	bltu	a0, a1, 1b

2:
	slli	t0, t0, 10

	la	sp, stacks + STACK_SIZE	# set the initial stack pointer to the end of the first stack space
	add	sp, sp, t0

	la	t0,	_trap_handler
	csrw	mtvec, t0

	# At the end of start_kernel, schedule() will call MRET to switch
	# to the first task, so we parepare the mstatus here.
	# Notice: It is best not to assume that the initial value of mstatus is
	# zero.
	# Set mstatus.MPP to 3, so we still run in Machine mode after MRET.
	# Set mstatus.MPIE to 1, so MRET will enable the interrupt.
	li	t0, 3 << 11 | 1 << 7
	csrs	mstatus, t0

	j start_kernel

wait:
	wfi
	j	wait


.balign
stacks:
	.skip STACK_SIZE * MAXNUM_CPU


# interrupts and exceptions while in machine mode come here
.section .text
.align 4
.globl _trap_handler
.globl _trap_exit
_trap_handler:
	csrrw	t6, mscratch, t6	# swap t6 and mscratch,
								# mscratch not be used load/restore instruction.
	SAVE_REG	t6				# save context registers

	# save actual t6 reg
	mv	t5, t6					# t5 point to the context of current
	csrr	t6, mscratch		# read t6 from the mscratch
	REG_S	t6, 30*SIZE_REG(t5) # save t6 use t5 as base

	csrr	t0, mepc			# save mepc to context of current task
	REG_S	t0,	31*SIZE_REG(t5)

	csrw	mscratch, t5		# read mscratch from t5

	# call the C trap handler
	csrr	a0,	mepc
	csrr	a1,	mcause
	csrr	a2, mscratch
	call	rv_trap_handler		# auipc, jalr
_trap_exit:
	csrw	mepc, a0			# handler will return the return addr via a0
	csrr	t6, mscratch		# mscratch not be used load/restore instruction

	RESTORE_REG	t6				# restore context reg
	mret

.end
